package nape.constraint;
import cx.FastList;
import cx.Algorithm;
import nape.geom.Vec2;
import nape.geom.VecMath;
import nape.phys.PhysObj;
import nape.space.Space;
import nape.util.FastMath;
import nape.util.NIntHash;
import nape.Const;
import nape.constraint.Constraint;
import nape.constraint.Anchor;
import nape.constraint.MPConsPair;





















class MultiSlideJoint extends Constraint {
	public var bodies:CxFastList(PhysObj);
	public var bodycnt:IntHash<Int>;
	
	public var cpairs:CxFastList(MPConsPair);
	public var apairs:CxFastList(Anchor);
	
	public var nMass:Float;
	public var bias:Float;
	public var jnAcc:Float;
	public var jnMax:Float;
	
	public var jointMin:Float;
	public var jointMax:Float;
	public var slack:Bool;
	public var scale:Float;
	
	
	
	public function new (objects:Array<PhysObj>,anchors:Array<Vec2>,?ratios:Array<Float>, min:Float,max:Float) {
		super();
		bodies  = new CxFastList(PhysObj)();
		bodycnt = new IntHash<Int>();
		cpairs  = new CxFastList(MPConsPair)();
		apairs  = new CxFastList(Anchor)();
		jointMin = min;
		jointMax = max;
		jnAcc = 0;
		
		if(ratios==null) {
			ratios = new Array<Float>();
			for(i in 0...(objects.length>>1)) ratios.push(1.0);
		}
		
		var bi = null;
		for(b in objects) {
			if(bodycnt.exists(b.id)) bodycnt.set(b.id,bodycnt.get(b.id)+1);
			else {
				bodycnt.set(b.id,1);
				bi = bodies.insert(bi,b);
			}
		}
		
		var i = 0;
		var ai = null;
		for(a in anchors) {
			var nv = new Vec2();
			var b = objects[i++];
			lanchor(a,b,nv.p);
			ai = apairs.insert(ai, new Anchor(nv,new Vec2(),b));
		}
		
		ai = apairs.begin();
		var ci = null;
		vec_new(delta);
		for(r in ratios) {
			var a1 = ai.elem(); var a2 = (ai=ai.next).elem(); ai=ai.next;
			var cp = new MPConsPair(a1,a2,r);
			ci = cpairs.insert(ci,cp);
		}
	}
	
	
	
	public override function preStep(dt:Float) {
		CxIter(a,apairs,{
			var la = a.la; var pa = a.pa; var b = a.b;
			ranchor(la.p,b,pa.p);
		});
		
		var tot_dist = 0.0;
		var inMass   = 0.0;
		CxIter(c,cpairs,{
			var a1 = c.a1; var b1 = a1.b; var p1 = a1.pa;
			var a2 = c.a2; var b2 = a2.b; var p2 = a2.pa;
			
			rdelta(b1,b2, p1.p,p2.p, c.n);
			var dist; delta_norm(c.n,dist);
			
			tot_dist += dist*c.coef;
			inMass += c.coef*c.coef*(KScalar2(b1,p1.p,c.n)*bodycnt.get(b1.id)
			                        +KScalar2(b2,p2.p,c.n)*bodycnt.get(b2.id));
		});
		
		nMass = if(inMass<Const.EPSILON) 0.0 else 1.0 / inMass;
		jnMax = maxForce*dt;
		
		var dist;
		if(tot_dist>jointMax)  {
			dist = jointMax  - tot_dist;
			bias = Calc.clamp(biasCoef*dist/dt,maxBias);
			slack = false;
			scale = 1;
		}else if(tot_dist<jointMin) {
			dist = tot_dist - jointMin;
			bias = Calc.clamp(biasCoef*dist/dt,maxBias);
			slack = false;
			scale = -1;
		} else {
			dist  = 0;
			jnAcc = 0;
			nMass = 0;
			bias = 0;
			slack = true;
		}
		
		if(bias!=0) {
			CxIter(c,cpairs,{
				var a1 = c.a1; var b1 = a1.b; var p1 = a1.pa;
				var a2 = c.a2; var b2 = a2.b; var p2 = a2.pa;
				
				var jn = jnAcc*c.coef*scale;
				AppImp(b1,b2,p1.p,p2.p,c.n,jn,0,);
			});
		}
		
		return false;
	}
	
	public override function applyImpulse() {
		if(slack) return false;
		
		var vdot = 0.0;
		vec_new(vr);
		CxIter(c,cpairs,{
			var a1 = c.a1; var b1 = a1.b; var p1 = a1.pa;
			var a2 = c.a2; var b2 = a2.b; var p2 = a2.pa;
			
			calc_relvel(b1,b2,p1.p,p2.p,vr,);
			vdot += vec_dot(vr,c.n)*c.coef;
		});
		
		var jn = nMass*(bias-vdot*scale);
		var jnOld = jnAcc; jnAcc += jn;
		if(breakable) { if(jnAcc*jnAcc>jnMax*jnMax) return true; }
		
		jnAcc = Calc.range(jnAcc,-jnMax,0);
		jn = jnAcc-jnOld;
		
		CxIter(c,cpairs,{
			var a1 = c.a1; var b1 = a1.b; var p1 = a1.pa;
			var a2 = c.a2; var b2 = a2.b; var p2 = a2.pa;
			
			var pjn = jn*c.coef*scale;
			AppImp(b1,b2,p1.p,p2.p,c.n,pjn,0,);
		});
		
		return false;
	}
	

	
	
	public override function impulse(obj:PhysObj,dst:Vec2) {
		if(slack) return;
		
		vec_set(dst.p,0,0);
		CxIter(c,cpairs,{
			var b1 = c.a1.b;
			var b2 = c.a2.b;
			if     (obj==b1) vec_subeq(dst.p,c.n,jnAcc*c.coef*scale);
			else if(obj==b2) vec_addeq(dst.p,c.n,jnAcc*c.coef*scale);
		});
	}
	
	
	
	public override function addToBodies() {
		CxIter(b,bodies,b.p_constraints.add(this));
	}
	public override function removeFromBodies() {
		CxIter(b,bodies,b.p_constraints.remove(this));
	}
	
	public override function wakeBodies(space:Space) {
		CxIter(b,bodies,space.wakeObject(b));
	}
	
	public override function bodyPairExists(a:PhysObj,b:PhysObj) {
		var fst = false; var snd = false;
		CxIter(x,bodies,{
			if(x==a) fst = true;
			if(x==b) snd = true;
			if(fst&&snd) return true;
		});
		return false;
	}
		
	
	public override function body_list() return bodies
	
	public override function anyBodiesAwake()
		return CxExists(b,bodies,!b.sleep)
	
	public override function disallowBodySleep() {
		CxIter(b,bodies,if(b.id!=0) b.evslp=0);
	}
}